#include "rclcpp/rclcpp.hpp"
#include <opencv2/opencv.hpp>
#include "sensor_msgs/msg/image.hpp"
#include <cv_bridge/cv_bridge.h>
using namespace cv;
using namespace std;

//发布者节点，发布图像消息
class ArmorPublisher : public rclcpp::Node{
public:
    ArmorPublisher(string name) : Node(name){
        //创建发布者话题
        sensor_img = this->create_publisher<sensor_msgs::msg::Image>("/sensor_img",10);
        //创建定时器
        timer_img = this->create_wall_timer(chrono::milliseconds(500),bind(&ArmorPublisher::timer_back,this));
    }
private:
    //声明发布者话题
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr sensor_img;
    //声明定时器
    rclcpp::TimerBase::SharedPtr timer_img;

    //创建发布函数
    void timer_back(){
        //输入图片
        Mat img = imread("/home/vboxuser/armor/src/armor_topic/src/3.png");
        //检测图片是否为空
        if(img.empty()){
            RCLCPP_WARN(this->get_logger(),"路径错误，请检查路径");
            return;
        }

        //将图片信息通过cv_bridge转为可传输
        auto picture = cv_bridge::CvImage(std_msgs::msg::Header(),"bgr8",img).toImageMsg();
        //发布信息
        sensor_img->publish(*picture);
    }
};

//主函数
int main(int argc,char **argv){
    //初始化参数
    rclcpp::init(argc,argv);
    //定义调用包的名称
    auto node = make_shared<ArmorPublisher>("armor_publisher");
    rclcpp::spin(node);
    rclcpp::shutdown();
}